#include "../ABB_functions_test.h"

#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h>

int sockfd;

//--Error message
void error(const char *msg)
{
    perror(msg);
    exit(0);
}

//--Connects to the specified port and IP
void CConnect::connect2Robot(int argc, char **argv)
{
    int portno;
    struct sockaddr_in serv_addr;
    struct hostent *server;

    if (argc < 3)
    {
        fprintf(stderr,"usage %s hostname port\n", argv[0]);
        exit(0);
    }
    portno = atoi(argv[2]);
    sockfd = socket(AF_INET, SOCK_STREAM, 0);
    mSocket = sockfd;
    if (sockfd < 0)
        error("ERROR opening socket");
    server = gethostbyname(argv[1]);
    if (server == NULL)
    {
        fprintf(stderr,"ERROR, no such host\n");
        exit(0);
    }
    bzero((char *) &serv_addr, sizeof(serv_addr));
    serv_addr.sin_family = AF_INET;
    bcopy((char *)server->h_addr,
          (char *)&serv_addr.sin_addr.s_addr,
          server->h_length);
    serv_addr.sin_port = htons(portno);
    if (connect(sockfd,(struct sockaddr *) &serv_addr,sizeof(serv_addr)) < 0)
        error("ERROR connecting");
}

//--Write to socket and then read the response
void CConnect::writeRead(char* command, bool Put)
{
    if(Put)
        std::cout << command << std::endl;

    write(sockfd,command,strlen(command));

    bzero(mBuffer,256);
    read(sockfd,mBuffer,255);
}
